/*
 *	Created by Adam Stroud
 *	September 9, 2014
 *
 */

#ifndef ATMEGA328P_H_
#define ATMEGA328P_H_

#include <vector>
#include <ctime>
#include <iostream>
#include <fstream>
#include <cmath>

#include "MX28.h"
#include "MotionStatus.h"
#include "FSR.h"


//TODO for some reason, comm only works when using ttyACM1, not ttyACM0
#define USB_SERIAL_PORT "/dev/ttyACM1" // (Linux Ubuntu Mac Book)
#define INERTIA_WAIT_CYCLES		1

#define FB_GYRO		1
#define RL_GYRO		2
#define FB_ACCEL	3
#define FB_ACCEL	4



using namespace Robot;
using namespace std;


class ATMega328p {
private:
	FILE *file;
	int port_fd;
	char sensor_buf[256];

	int inertia_counter;


//	int gyroZ;


//	int accelZ;

//	int cmpsX;
//	int cmpsY;
//	int cmpsZ;

	static ATMega328p* m_UniqueInstance;


public:
	int gyroX;
	int gyroY;


	int accelX;
	int accelY;

	ATMega328p();
	virtual ~ATMega328p();

	static ATMega328p* GetInstance() { return m_UniqueInstance; }

	void initialize();
	int init_serial_input (char*);
	void read_serial_xyz(int);
	void parseXYZ(std::string);
	void UpdateInertialMeasurements();
//	int getFB_GYRO();
//	int getRL_GYRO();
//	int getFB_ACCEL();
//	int getRL_ACCEL();


};

#endif /* ATMEGA328P_H_ */
